/**
 * @file tank_node.cpp
 * @brief 两轮差动小车
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-05-19
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#include <iostream>
#include <string>
#include <sstream>

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"

#include <nav_msgs/Odometry.h>  
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>

serial::Serial ser;

double max_linear_x = 1.296;	//   m/s
double max_angular_z = 6.3529; 	// rad/s

std::vector<std::string> split(const std::string& str, const char pattern){
	std::vector<std::string> res;
	std::stringstream input(str);
	std::string temp;
	while(getline(input, temp, pattern)){
		res.push_back(temp);
	}	
	return res;
}

template <class Type>
Type stringToNum(const std::string& str)
{
  std::istringstream iss(str);
  Type num;
  iss >> num;
  return num;
}

/**
 * @brief 速度指令回调函数
 * @param  cmd_vel          速度指令
 */
void cmd_callback(const geometry_msgs::Twist::ConstPtr& cmd_vel)
{
	double L = 0.408; 				// m
	double wheel_max_rpm = 1000; 	// 车轮最大转速 rpm
	double linear_x = cmd_vel->linear.x;
	double angular_z = cmd_vel->angular.z;

	// double vel_r = (2 * linear_x + angular_z * L) / 2.0;
	// double vel_l = linear_x - angular_z * L / 2.0;

	// // 转速转换为车轮线速度的比例系数
	// double coeffi = 1 / 60.0 * 20.0 * 12.0 * 1.08 / 1000.0;
	// double vel_r_rpm = vel_r / coeffi;
	// double vel_l_rpm = vel_l / coeffi;

	// int vel_r_rpm_norm = vel_r_rpm / wheel_max_rpm * 1000;
	// int vel_l_rpm_norm = vel_l_rpm / wheel_max_rpm * 1000;

	linear_x  = linear_x  > max_linear_x  ? max_linear_x  : (linear_x  < -max_linear_x  ? -max_linear_x  : linear_x );
	angular_z = angular_z > max_angular_z ? max_angular_z : (angular_z < -max_angular_z ? -max_angular_z : angular_z);

	double linear_x_setting = linear_x / max_linear_x * 1000.0;
	double angular_z_setting = angular_z / max_angular_z * 1000.0;

	// 通道1： 角速度 通道2： 线速度
	std::stringstream cmd;
	cmd << "!M " << (int)(angular_z_setting) << " " << (int)(-linear_x_setting) << "\r";
	ROS_INFO("%s", cmd.str().c_str());

	ser.write(cmd.str());
	
	// std::string receive;
	// do{
	// 	ser.read(ser.available());
	// 	ser.write(cmd.str());
	// 	while(!ser.available()); // 等待回复
	// 	receive = ser.readline();
	// }while(receive != "+\r"); // 判断是否发送成功
	// ROS_INFO("send ok");
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tank_node");
	ros::NodeHandle private_nh("~");
	ros::NodeHandle n;

	std::string serial_port;
	int serial_baudrate;
	private_nh.param("serial_port", serial_port, std::string("/dev/ttyUSB1"));
	private_nh.param("serial_baudrate", serial_baudrate, 115200);

	ros::Publisher vel_pub = private_nh.advertise<geometry_msgs::Twist>("vel", 10);
	ros::Subscriber sub = n.subscribe("/cmd_vel", 10, cmd_callback);

	ros::Rate sleep_time(0.1);
	sleep_time.sleep();

    // 打开串口
	try
	{
		ser.setPort(serial_port);
		ser.setBaudrate(serial_baudrate);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		ser.setTimeout(to);
		ser.open();
	}
	catch(serial::IOException &e)
	{
		ROS_INFO_STREAM("Failed to open port: tank");
		return -1;
	}
	ROS_INFO_STREAM("Succeed to open port: tank" );

	// 清空串口缓存区
	ser.read(ser.available());

    ros::Rate loop_rate(20);

    while(ros::ok()){

		// 清空串口缓存区
		ser.read(ser.available());
		std::string cmd = "?S\r";
		ser.write(cmd);
		// while(!ser.available()); // 等待串口数据
		std::string data_receice = ser.readline(65535, "\r");
		data_receice = ser.readline(65535, "\r");

		double vel_l_mps = 0, vel_r_mps = 0;
		if(data_receice != ""){
			try{
				// 格式 S=vv:vv
				std::vector<std::string> result0 = split(data_receice, '=');
				// ROS_INFO("%s", data_receice.c_str());
				if(result0[0] == "S"){
					std::vector<std::string> result1 = split(result0[1], ':');
					vel_r_mps =  (stringToNum<int>(result1[0])) / 60.0 * 20.0 * 12.0 * 1.08 / 1000.0;
					vel_l_mps = -(stringToNum<int>(result1[1])) / 60.0 * 20.0 * 12.0 * 1.08 / 1000.0;

					// 差动小车运动学解算
					double L = 0.408; // 两轮间距
					geometry_msgs::Twist vel;
					vel.linear.x = (vel_l_mps + vel_r_mps) / 2.0;
					vel.angular.z = (vel_r_mps - vel_l_mps) / L; // 逆时针为正

					vel_pub.publish(vel);
					// ROS_INFO("publish ok");
				}

			}
			catch(...){}
		}


        ros::spinOnce();
        loop_rate.sleep();
    }

}